#include "ymbot_e_control/HOFA.hpp"
#include <Eigen/Dense>
#include <GLFW/glfw3.h>
#include <algorithm> // for std::clamp
#include <cmath>
#include <deque>
#include <iostream>
#include <mujoco/mujoco.h>
#include <ros/package.h>
#include <vector>
#include <ros/ros.h>
#include <tf/tf.h>
#include <fstream>


using namespace std;

class MujocoSimulation {
  public:
    mjModel* m = nullptr; // MuJoCo模型
    mjData* d = nullptr;  // MuJoCo数据
    mjvCamera cam;        // 相机
    mjvOption opt;        // 可视化选项
    mjvScene scn;         // 场景
    mjrContext con;       // 自定义GPU上下文

    bool button_left = false;   // 左键状态
    bool button_middle = false; // 中键状态
    bool button_right = false;  // 右键状态
    double lastx = 0;           // 上一次X坐标
    double lasty = 0;           // 上一次Y坐标

    MujocoSimulation() {
        // 初始化MuJoCo模型和数据
        if (!glfwInit()) {
            mju_error("Could not initialize GLFW");
        }
    }

    ~MujocoSimulation() {
        // 释放所有的MuJoCo资源
        mjv_freeScene(&scn);
        mjr_freeContext(&con);
        mj_deleteData(d);
        mj_deleteModel(m);
    }

    // 加载模型
    bool loadModel(const char* filename) {
        char error[1000] = "Could not load binary model";
        if (std::strlen(filename) > 4 && !std::strcmp(filename + std::strlen(filename) - 4, ".mjb")) {
            m = mj_loadModel(filename, 0); // 加载二进制模型
        }
        else {
            m = mj_loadXML(filename, 0, error, 1000); // 加载XML模型
        }
        if (!m) {
            mju_error("Load model error: %s", error);
            return false;
        }
        d = mj_makeData(m); // 创建模拟数据
        return true;
    }

    // 键盘回调
    static void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods) {
        auto* sim = reinterpret_cast<MujocoSimulation*>(glfwGetWindowUserPointer(window));
        if (act == GLFW_PRESS && key == GLFW_KEY_BACKSPACE) {
            mj_resetData(sim->m, sim->d); // 重置模拟数据
            mj_forward(sim->m, sim->d);   // 更新模型数据
        }
    }

    // 鼠标按钮回调
    static void mouse_button(GLFWwindow* window, int button, int act, int mods) {
        auto* sim = reinterpret_cast<MujocoSimulation*>(glfwGetWindowUserPointer(window));
        sim->button_left = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS);
        sim->button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS);
        sim->button_right = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS);
        glfwGetCursorPos(window, &sim->lastx, &sim->lasty); // 获取当前鼠标位置
    }

    // 鼠标移动回调
    static void mouse_move(GLFWwindow* window, double xpos, double ypos) {
        auto* sim = reinterpret_cast<MujocoSimulation*>(glfwGetWindowUserPointer(window));

        if (!sim->button_left && !sim->button_middle && !sim->button_right) {
            return;
        }

        double dx = xpos - sim->lastx; // 计算鼠标X轴位移
        double dy = ypos - sim->lasty; // 计算鼠标Y轴位移
        sim->lastx = xpos;
        sim->lasty = ypos;

        int width, height;
        glfwGetWindowSize(window, &width, &height); // 获取窗口尺寸

        bool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS ||
                          glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS);

        mjtMouse action;
        if (sim->button_right) {
            action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;
        }
        else if (sim->button_left) {
            action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;
        }
        else {
            action = mjMOUSE_ZOOM;
        }

        mjv_moveCamera(sim->m, action, dx / height, dy / height, &sim->scn, &sim->cam); // 移动相机
    }

    // 滚轮回调
    static void scroll(GLFWwindow* window, double xoffset, double yoffset) {
        auto* sim = reinterpret_cast<MujocoSimulation*>(glfwGetWindowUserPointer(window));
        mjv_moveCamera(sim->m, mjMOUSE_ZOOM, 0, -0.05 * yoffset, &sim->scn, &sim->cam); // 缩放相机视角
    }
};


struct Cmd {
    static constexpr double vx = 0.4;
    static constexpr double vy = 0.0;
    static constexpr double dyaw = 0.0;
};


struct EnvConfig {
    static constexpr int frame_stack = 15;               // 帧堆叠的数量
    static constexpr int c_frame_stack = 3;              // 另一个帧堆叠数量
    static constexpr int num_single_obs = 47;            // 单一观测的数量
    static constexpr int single_num_privileged_obs = 73; // 单一特权观测数量
    static constexpr int num_actions = 30;               // 动作的数量
    static constexpr int num_envs = 4096;                // 环境数量
    static constexpr int episode_length_s = 24;          // 片段时长（秒）

    // 计算成员变量
    static constexpr int num_observations = frame_stack * num_single_obs;                // 总观测数量
    static constexpr int num_privileged_obs = c_frame_stack * single_num_privileged_obs; // 特权观测数量
};


struct RobotConfig {
    // static constexpr std::array<double, 12> kps = {200, 200, 350, 350, 15, 15,
    //                                                200, 200, 350, 350, 15, 15}; // 比例控制器增益 (P)
    // static constexpr std::array<double, 12> kds = {10, 10, 10, 10, 10, 10,
    //                                                10, 10, 10, 10, 10, 10}; // 微分控制器增益 (D)


    static constexpr std::array<double, 30> kps = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
                                                   100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
                                                   100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
                                                   200.0, 200.0, 200.0, 200.0, 200.0, 200.0}; // 比例控制器增益 (P)

    static constexpr std::array<double, 30> kds = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                   0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                   0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // 比例控制器增益 (D)

    static constexpr double tau_limit = 200.0; // 力矩限制
};


// struct ControlConfig {
//     // PD Drive parameters
//     static constexpr std::array<std::pair<const char*, double>, 5> stiffness = {
//         {{"leg_roll", 200.0}, {"leg_pitch", 350.0}, {"leg_yaw", 200.0}, {"knee", 350.0}, {"ankle", 15.0}}}; // 刚度

//     static constexpr std::array<std::pair<const char*, double>, 5> damping = {
//         {{"leg_roll", 10.0}, {"leg_pitch", 10.0}, {"leg_yaw", 10.0}, {"knee", 10.0}, {"ankle", 10.0}}}; // 阻尼

//     static constexpr double action_scale = 0.25; // 动作缩放因子
//     // static constexpr int decimation = 10;        // 降采样
// };


struct NormalizationConfig {
  public:
    struct ObsScales {
        static constexpr double lin_vel = 2.0;
        static constexpr double ang_vel = 1.0;
        static constexpr double dof_pos = 1.0;
        static constexpr double dof_vel = 0.05;
        static constexpr double quat = 1.0;
        static constexpr double height_measurements = 5.0;
    };

    static constexpr double clip_observations = 18.0;
    static constexpr double clip_actions = 18.0;

    ObsScales obs_scales;
};

struct SimConfig {
    // 静态常量
    static constexpr double sim_duration = 60.0; // 仿真总时长
    static constexpr double dt = 0.001;          // 仿真步长
    static constexpr int decimation = 10;        // 降采样因子

    std::string mujoco_model_path; // MuJoCo模型路径

    // 构造函数
    SimConfig(const std::string& model_path) {
        mujoco_model_path = model_path;
    }
};

struct Config {
    // 使用结构体名称直接访问静态成员
    EnvConfig env;            // 环境配置
    RobotConfig robot_config; // 机器人配置
    // ControlConfig control;             // 控制相关配置
    NormalizationConfig normalization; // 归一化配置

    // 直接定义 SimConfig，因其需要参数
    SimConfig sim_config; // MuJoCo仿真配置

    // 构造函数
    Config(const std::string& model_path) : sim_config(model_path) {} // 初始化 SimConfig
};


std::vector<double> quaternion_to_euler_array(const std::vector<double>& quat) {
    // 确保四元数的格式是 [x, y, z, w]
    double x = quat[1];
    double y = quat[2];
    double z = quat[3];
    double w = quat[0];

    // 滚转（x 轴旋转）
    double t0 = +2.0 * (w * x + y * z);
    double t1 = +1.0 - 2.0 * (x * x + y * y);
    double roll_x = std::atan2(t0, t1);

    // 俯仰（y 轴旋转）
    double t2 = +2.0 * (w * y - z * x);
    t2 = std::clamp(t2, -1.0, 1.0); // 限制 t2 在 -1 到 1 之间
    double pitch_y = std::asin(t2);

    // 偏航（z 轴旋转）
    double t3 = +2.0 * (w * z + x * y);
    double t4 = +1.0 - 2.0 * (y * y + z * z);
    double yaw_z = std::atan2(t3, t4);

    // 返回滚转、俯仰、偏航的向量（以弧度表示）
    std::vector<double> euler_angle = {roll_x, pitch_y, yaw_z};

    // 调整角度范围
    for (auto& angle : euler_angle) {
        if (angle > M_PI) {
            angle -= 2 * M_PI;
        }
    }

    return euler_angle;
}


// 使用函数来获取机器人状态，定义返回类型为 std::tuple，包含多个 std::vector<double>
std::tuple<std::vector<double>,
           std::vector<double>,
           std::vector<double>,
           std::vector<double>,
           std::vector<double>,
           std::vector<double>>
get_robot_state(const mjData& data, const mjModel* model) {
    // 显示每个 qpos 元素对应的关节名称及其值
    // std::cout << "关节位置 (qpos) 信息：" << std::endl;
    // for (int j = 0; j < model->njnt; ++j) {
    //     const char* joint_name = mj_id2name(model, mjOBJ_JOINT, j);
    //     if (joint_name) {
    //         int qpos_index = model->jnt_qposadr[j];
    //         if (qpos_index >= 0 && qpos_index < model->nq) {
    //             double qpos_value = data.qpos[qpos_index];
    //             std::cout << "  " << joint_name << ": " << qpos_value << std::endl;
    //         }
    //         else {
    //             std::cerr << "  错误：关节 '" << joint_name << "' 的 qpos 索引 " << qpos_index << " 超出范围。"
    //                       << std::endl;
    //         }
    //     }
    //     else {
    //         std::cerr << "  错误：无法获取关节索引 " << j << " 的名称。" << std::endl;
    //     }
    // }


    // 关节位置（30个元素）
    std::vector<double> q;
    for (int i = 0; i < 30; ++i) {
        q.push_back(data.qpos[i]);
    }

    // 关节速度（30个元素）
    std::vector<double> dq;
    for (int i = 6; i < 6 + 30; ++i) {
        dq.push_back(data.qvel[i]);
    }

    // 关节加速度（30个元素）
    std::vector<double> ddq;
    for (int i = 6; i < 6 + 30; ++i) {
        ddq.push_back(data.qacc[i]);
    }

    // 从传感器获取四元数 (MuJoCo 四元数格式为 w, x, y, z)
    std::vector<double> quat;
    int orientation_id = mj_name2id(model, mjOBJ_SENSOR, "orientation");
    int quat_start_idx = model->sensor_adr[orientation_id]; // 获取四元数在 sensordata 中的起始索引
    for (int i = quat_start_idx; i < quat_start_idx + 4; ++i) {
        quat.push_back(data.sensordata[i]);
    }

    // // 从仿真引擎中获取四元数: w x y z
    // std::vector<double> quat;
    // for (int i = 3; i < 7; ++i) {
    //     quat.push_back(data.qpos[i]);
    // }


    // 获取线速度 (qvel 的前三个元素，表示浮动基座的线速度)
    std::vector<double> v;
    for (int i = 0; i < 3; ++i) {
        v.push_back(data.qvel[i]);
    }

    // 获取角速度 (qvel 的后三个元素，表示浮动基座的角速度)
    std::vector<double> omega;
    for (int i = 3; i < 6; ++i) {
        omega.push_back(data.qvel[i]);
    }

    // 返回多个 std::vector<double>，分别包含关节位置、关节速度、四元数、线速度、角速度
    return std::make_tuple(q, dq, ddq, quat, v, omega);
}

// PD控制函数，计算力矩输出
std::vector<double> pd_control(const std::vector<double>& target_q,
                               const std::vector<double>& q,
                               const std::array<double, 30>& kp,
                               const std::vector<double>& target_dq,
                               const std::vector<double>& dq,
                               const std::array<double, 30>& kd) {
    // 确保输入向量的大小相同
    std::size_t n = target_q.size();
    std::vector<double> torque(n);

    for (std::size_t i = 0; i < n; i++) {
        torque[i] = (target_q[i] - q[i]) * kp[i] + (target_dq[i] - dq[i]) * kd[i];
    }

    return torque;
}

void run_mujoco(const Config& cfg) {
    //创建 mujoco 仿真对象，包含大部分仿真内容
    MujocoSimulation mj_sim;
    if (!mj_sim.loadModel(cfg.sim_config.mujoco_model_path.c_str())) {
        return;
    }
    mj_sim.m->opt.timestep = 0.001;

    // GLFWwindow* window = glfwCreateWindow(1200, 900, "Demo", NULL, NULL);
    GLFWwindow* window = glfwCreateWindow(2560, 1440, "Demo", NULL, NULL);
    glfwMakeContextCurrent(window);
    glfwSwapInterval(1); // 开启垂直同步

    mjv_defaultCamera(&mj_sim.cam);                          // 初始化相机
    mjv_defaultOption(&mj_sim.opt);                          // 初始化可视化选项
    mjv_defaultScene(&mj_sim.scn);                           // 初始化场景
    mjr_defaultContext(&mj_sim.con);                         // 初始化GPU上下文
    mjv_makeScene(mj_sim.m, &mj_sim.scn, 2000);              // 创建场景
    mjr_makeContext(mj_sim.m, &mj_sim.con, mjFONTSCALE_150); // 创建渲染上下文

    glfwSetWindowUserPointer(window, &mj_sim);               // 设置窗口的用户指针为当前对象
    glfwSetKeyCallback(window, mj_sim.keyboard);             // 设置键盘回调
    glfwSetCursorPosCallback(window, mj_sim.mouse_move);     // 设置鼠标移动回调
    glfwSetMouseButtonCallback(window, mj_sim.mouse_button); // 设置鼠标按钮回调
    glfwSetScrollCallback(window, mj_sim.scroll);            // 设置滚轮回调

    // 计时变量，控制帧率
    int count_lowlevel = 0;

    // 准备文件，导出仿真数据，方便画图分析
    std::string package_path = ros::package::getPath("ymbot_e_control");
    std::string file_path = package_path + "/data/record_data.txt";

    // 打开文件，使用截断模式覆盖写入
    std::ofstream outfile(file_path, std::ios::out | std::ios::trunc);
    // 检查文件是否成功打开
    if (!outfile) {
        std::cerr << "Error opening file at: " << file_path << std::endl;
        return;
    }


    // 准备非线性干扰观测器
    Eigen::VectorXd diag_values_mp(30);
    diag_values_mp.segment<4>(0) << 0.0, 0.0, 0.0, 0.0;
    diag_values_mp.segment<7>(4) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
    diag_values_mp.segment<7>(11) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
    diag_values_mp.segment<6>(18) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
    // diag_values_mp.segment<6>(24) << 200.0, 5.0, 5.0, 5.0, 5.0, 500.0; // 原始版本 ndo 的参数
    diag_values_mp.segment<6>(24) << 0.001, 0.001, 0.001, 0.001, 0.0015, 0.001; // 降阶后的ndo，要从很小的值开始调整


    Eigen::MatrixXd mp = diag_values_mp.asDiagonal();
    NonlinearDisturbanceObserver ndo(mp);

    // 准备HOFA控制器
    std::string urdf_path = ros::package::getPath("ymbot_e_description") + "/urdf/ymbot_e_description.urdf";
    CoefficientMatrix cm(urdf_path);
    cm.print_model_info();
    HOFA hofa(30);
    Eigen::VectorXd diag_values(60); // 矩阵F的对角线元素不能全部一样，否则会导致V为奇异矩阵从而无法求逆
    // diag_values.head(30).setConstant(-150);
    // diag_values.tail(30).setConstant(-180);
    diag_values.segment<4>(0) << 150.0, 150.0, 150.0, 150.0;
    diag_values.segment<7>(4) << 150.0, 150.0, 150.0, 150.0, 150.0, 150.0, 150.0;
    diag_values.segment<7>(11) << 150.0, 150.0, 150.0, 150.0, 150.0, 150.0, 150.0;
    diag_values.segment<6>(18) << 150.0, 150.0, 150.0, 150.0, 150.0, 150.0;
    diag_values.segment<6>(24) << 2000.0, 2000.0, 2000.0, 4000.0, 2000.0, 4000.0; //

    diag_values.segment<4>(30) << 200.0, 200.0, 200.0, 200.0;
    diag_values.segment<7>(34) << 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0;
    diag_values.segment<7>(41) << 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0;
    diag_values.segment<6>(48) << 200.0, 200.0, 200.0, 200.0, 200.0, 200.0;
    diag_values.segment<6>(54) << 500.0, 500.0, 500.0, 1000.0, 200.0, 1000.0; //
    Eigen::MatrixXd F = diag_values.asDiagonal();
    Eigen::MatrixXd Z = Eigen::MatrixXd::Zero(30, 30 * 2);
    Z.block(0, 0, 30, 30) = 2 * Eigen::MatrixXd::Identity(30, 30);
    Z.block(0, 30, 30, 30) = Eigen::MatrixXd::Identity(30, 30);


    // 设置速度因子, 更加方便
    double speed_factor = 1.0;
    double screen_refresh_rate = 100.0;
    double steps = 1.0 / screen_refresh_rate / mj_sim.m->opt.timestep * speed_factor;

    // 设置初始关节角度，测试用
    // mj_sim.d->qpos[24] = -0.0 / 180.0 * M_PI;
    // mj_sim.d->qpos[25] = -0.0 / 180.0 * M_PI;
    // mj_sim.d->qpos[26] = -0.0 / 180.0 * M_PI;
    // mj_sim.d->qpos[27] = -0.0 / 180.0 * M_PI;
    // mj_sim.d->qpos[28] = -0.0 / 180.0 * M_PI;
    // mj_sim.d->qpos[29] = -0.0 / 180.0 * M_PI;

    //设置目标关节角度
    std::vector<double> target_q = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 0.0, 0.0,
                                    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, -0.0, -0.0, -0.0, -0.0, 0.0}; // 角度
    for (auto& angle_deg : target_q) {
        angle_deg = angle_deg * M_PI / 180.0;
    }
    std::vector<double> target_dq(cfg.env.num_actions, 0.0);
    const double amplitude = 8.0 / 180.0 * M_PI; // 振幅(度)
    const double frequency = 0.5;                // 频率（Hz）
    // const double sim_step_time = 1.0 / screen_refresh_rate; // 仿真步长，单位：秒


    while (!glfwWindowShouldClose(window)) {
        double current_time = glfwGetTime();
        for (int n = 0; n < steps; n++) {
            std::cout << "count_lowlevel" << count_lowlevel << std::endl;

            auto [q, dq, ddq, quat, v, omega] = get_robot_state(*mj_sim.d, mj_sim.m); // 获取机器人的状态数据

            double t = count_lowlevel * mj_sim.m->opt.timestep;
            // target_q[18] = amplitude * sin(2 * M_PI * frequency * t);
            // target_q[19] = amplitude * sin(2 * M_PI * frequency * t);
            // target_q[20] = amplitude * sin(2 * M_PI * frequency * t);
            // target_q[21] = amplitude * sin(2 * M_PI * frequency * t);
            // target_q[22] = amplitude * sin(2 * M_PI * frequency * t);
            // target_q[23] = amplitude * sin(2 * M_PI * frequency * t);

            target_q[24] = amplitude * sin(2 * M_PI * frequency * t);
            target_q[25] = amplitude * sin(2 * M_PI * frequency * t);
            target_q[26] = amplitude * sin(2 * M_PI * frequency * t);
            target_q[27] = amplitude * sin(2 * M_PI * frequency * t);
            target_q[28] = amplitude * sin(2 * M_PI * frequency * t);
            target_q[29] = amplitude * sin(2 * M_PI * frequency * t);


            // 计算PD控制力矩
            // std::vector<double> tau_pid =
            //     pd_control(target_q, q, cfg.robot_config.kps, target_dq, dq, cfg.robot_config.kds);
            // Eigen::VectorXd u_pid = Eigen::Map<Eigen::VectorXd>(tau_pid.data(), tau_pid.size());

            // 计算控制器和观测器需要的输入
            Eigen::VectorXd qe = Eigen::Map<const Eigen::VectorXd>(q.data(), q.size()) -
                                 Eigen::Map<const Eigen::VectorXd>(target_q.data(), target_q.size());
            Eigen::VectorXd dqe = Eigen::VectorXd::Zero(dq.size()); // 目标速度信号无法赋值，一旦给值，控制器就不稳定
            Eigen::VectorXd dq_vec = Eigen::Map<const Eigen::VectorXd>(dq.data(), dq.size());
            Eigen::VectorXd ddq_vec = Eigen::Map<const Eigen::VectorXd>(ddq.data(), ddq.size());

            cm.compute_inertia_matrix(qe);
            cm.compute_coriolis_matrix(qe, dqe);
            cm.compute_gravity_vector(qe);

            // ndo.estimate_d(cm.M, cm.C, cm.G, dq_vec, u_pid, mj_sim.m->opt.timestep); // 计算d的估计值
            // ndo.simple_d(cm.M, cm.C, cm.G, dq_vec, ddq_vec, u_pid, mj_sim.m->opt.timestep); // 计算d的估计值
            // u_pid += ndo.d_estimated; // 加上非线性干扰观测器的估计值
            // std::vector<double> tau(u_pid.data(), u_pid.data() + u_pid.size());

            // hofa.construct_controller(F, Z, cm.M, cm.C, cm.G, qe, dqe, Eigen::VectorXd::Zero(30)); // 构建 HOFA控制器
            hofa.construct_controller(F, Z, cm.M, cm.C, cm.G, qe, dqe, ndo.d_estimated); // 构建 HOFA控制器
            ndo.estimate_d(cm.M, cm.C, cm.G, dq_vec, hofa.u, mj_sim.m->opt.timestep);    // 计算d的估计值
            // ndo.simple_d(cm.M, cm.C, cm.G, dq_vec, ddq_vec, hofa.u, mj_sim.m->opt.timestep); // 计算d的估计值
            std::vector<double> tau(hofa.u.data(), hofa.u.data() + hofa.u.size());

            // 打印各参数的维数
            // ROS_INFO_STREAM("qe: " << qe.transpose());
            // ROS_INFO_STREAM("qe: " << dqe.transpose());
            // ROS_INFO_STREAM("=== cm.M 矩阵 ===\n" << cm.M);
            // ROS_INFO_STREAM("=== cm.C 矩阵 ===\n" << cm.C);
            // ROS_INFO_STREAM("=== cm.G 矩阵 ===\n" << cm.G);
            // ROS_INFO_STREAM("=== construct_controller 参数维数 ===");
            // ROS_INFO_STREAM("F dimensions: " << F.rows() << " x " << F.cols());
            // ROS_INFO_STREAM("Z dimensions: " << Z.rows() << " x " << Z.cols());
            // ROS_INFO_STREAM("M dimensions: " << cm.M.rows() << " x " << cm.M.cols());
            // ROS_INFO_STREAM("C dimensions: " << cm.C.rows() << " x " << cm.C.cols());
            // ROS_INFO_STREAM("G dimensions: " << cm.G.rows() << " x " << cm.G.cols());
            // ROS_INFO_STREAM("qe size: " << qe.size());
            // ROS_INFO_STREAM("dqe size: " << dqe.size());
            // ROS_INFO_STREAM("uc: " << hofa.uc.transpose());
            // ROS_INFO_STREAM("uf: " << hofa.uf.transpose());


            // 限制力矩范围
            for (auto& t : tau) {
                t = std::clamp(t, -cfg.robot_config.tau_limit, cfg.robot_config.tau_limit);
            }

            // 应用控制力矩
            for (std::size_t i = 0; i < tau.size(); ++i) {
                mj_sim.d->ctrl[i] = tau[i];
                // cout << tau[i] << ", ";
            }
            // cout << endl;

            mj_step(mj_sim.m, mj_sim.d);

            count_lowlevel++;

            /************************** 导出数据 **********************/
            // 导出 q
            for (const auto& val : q) {
                outfile << val << ",";
            }

            // 导出 target_q
            for (const auto& val : target_q) {
                outfile << val << ",";
            }

            // 导出差值 (q - target_q)
            for (std::size_t i = 0; i < q.size(); ++i) {
                double difference = q[i] - target_q[i];
                outfile << difference << ",";
            }

            // 导出 d_estimated
            for (const auto& val : ndo.d_estimated) {
                outfile << val << ",";
            }

            // 导出 d_simple
            for (const auto& val : ndo.d_simple) {
                outfile << val << ",";
            }

            // // 导出 qe
            // for (const auto& val : qe) {
            //     outfile << val << ",";
            // }


            // // 导出 dq
            // for (const auto& val : dq) {
            //     outfile << val << ",";
            // }


            // // 导出 dqe
            // for (const auto& val : dqe) {
            //     outfile << val << ",";
            // }


            // // 导出 quat
            // for (const auto& val : quat) {
            //     outfile << val << ",";
            // }

            // // 导出 v
            // for (const auto& val : v) {
            //     outfile << val << ",";
            // }

            // // 导出 omega
            // for (const auto& val : omega) {
            //     outfile << val << ",";
            // }

            // // 导出 tua
            // for (const auto& val : tau) {
            //     outfile << val << ",";
            // }


            // 导出 tau 并按照每行 4、7、7、6、6 个元素的格式输出，同时在相邻两次输出之间添加一个空行
            const std::array<int, 5> group_sizes = {4, 7, 7, 6, 6};
            int pos = 0;
            // 遍历每个组的大小
            for (const auto& group_size : group_sizes) {
                for (int i = 0; i < group_size; ++i, ++pos) {
                    if (pos < tau.size()) {
                        std::cout << tau[pos] << ", ";
                        // std::cout << tau[pos] << ", ";
                    }
                }
                std::cout << std::endl; // 打印到控制台换行
            }
            std::cout << ndo.d_estimated.transpose() << std::endl;
            // 在两次输出之间添加一个空行
            std::cout << std::endl;

            outfile << std::endl;
        }

        // 渲染仿真
        mjrRect viewport = {0, 0, 0, 0};
        glfwGetFramebufferSize(window, &viewport.width, &viewport.height); // 获取帧缓冲尺寸

        // 更新场景
        mjv_updateScene(mj_sim.m, mj_sim.d, &mj_sim.opt, nullptr, &mj_sim.cam, mjCAT_ALL, &mj_sim.scn);

        // 渲染场景
        mjr_render(viewport, &mj_sim.scn, &mj_sim.con);

        // 交换缓冲区并处理事件
        glfwSwapBuffers(window);
        glfwPollEvents();
    }
}

int main(int argc, char** argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "mujoco_test");
    ros::NodeHandle nh;

    // 从参数服务器获取 model_path 和 is_rough_terrain 参数
    std::string controller_path;
    std::string model_path;


    // 获取 model_path 参数，若没有指定地址或者打不开地址中指定的文件，则提示错误并退出
    if (nh.getParam("model_path", model_path)) {
        std::cout << "Loaded model path from parameter server: " << model_path << std::endl;

        // 验证模型文件是否存在
        std::ifstream file(model_path);
        if (!file.is_open()) {
            std::cerr << "Model file does not exist at: " << model_path << std::endl;
            return 1;
        }
    }
    else {
        std::cerr << "Failed to get 'model_path' from parameter server." << std::endl;
        return 1;
    }


    // 实例化配置参数
    Config config(model_path);

    // 运行 MuJoCo
    run_mujoco(config);

    // ros::spin(); // 若需要让节点持续运行，否则根据实际需求调整

    return 0;
}